import os
# import rosbag
import numpy as np

# 常量定义
RE = 6378137.0
FLATTENING = 0.00335281066475
E_SQR = 0.00669437999014
M_PI = 3.14159265358979323846
DEG2RAD = (M_PI / 180.0)
e2 = FLATTENING * (2.0 - FLATTENING)

def sincos(theta):
    p_s_theta = np.sin(theta)
    p_c_theta = np.cos(theta)
    return p_s_theta, p_c_theta

def wsg84_to_ecef(lat, lon, alt):
    s_lat, c_lat = sincos(lat * DEG2RAD)
    s_lon, c_lon = sincos(lon * DEG2RAD)
    N = RE / np.sqrt(1 - E_SQR * s_lat * s_lat)
    x = (N + alt) * c_lat * c_lon
    y = (N + alt) * c_lat * s_lon
    z = (N * (1 - E_SQR) + alt) * s_lat
    return np.array([[x], [y], [z]])

def s_transform_lat(x, y):
    ret = -100.0 + 2.0 * x + 3.0 * y + 0.2 * y * y + 0.1 * x * y + 0.2 * np.sqrt(abs(x))
    ret += (20.0 * np.sin(6.0 * x * np.pi) + 20.0 * np.sin(2.0 * x * np.pi)) * 2.0 / 3.0
    ret += (20.0 * np.sin(y * np.pi) + 40.0 * np.sin(y / 3.0 * np.pi)) * 2.0 / 3.0
    ret += (160.0 * np.sin(y / 12.0 * np.pi) + 320 * np.sin(y * np.pi / 30.0)) * 2.0 / 3.0
    return ret

def s_transform_lon(x, y):
    ret = 300.0 + x + 2.0 * y + 0.1 * x * x + 0.1 * x * y + 0.1 * np.sqrt(abs(x))
    ret += (20.0 * np.sin(6.0 * x * np.pi) + 20.0 * np.sin(2.0 * x * np.pi)) * 2.0 / 3.0
    ret += (20.0 * np.sin(x * np.pi) + 40.0 * np.sin(x / 3.0 * np.pi)) * 2.0 / 3.0
    ret += (150.0 * np.sin(x / 12.0 * np.pi) + 300.0 * np.sin(x / 30.0 * np.pi)) * 2.0 / 3.0
    return ret

def s_transform(lat, lon):
    dLat = s_transform_lat(lon - 105.0, lat - 35.0)
    dLon = s_transform_lon(lon - 105.0, lat - 35.0)
    radLat = lat / 180.0 * np.pi
    magic = np.sin(radLat)
    magic = 1 - e2 * magic * magic
    sqrtMagic = np.sqrt(magic)
    dLat = (dLat * 180.0) / ((RE * (1 - e2)) / (magic * sqrtMagic) * np.pi)
    dLon = (dLon * 180.0) / (RE / sqrtMagic * np.cos(radLat) * np.pi)
    mgLat = lat + dLat
    mgLon = lon + dLon
    return mgLat, mgLon

def wgs84_to_gcj02(lat, lon):
    return s_transform(lat, lon)

def check_bag_files_and_extract_data(folder_path):
    results = {}

    for path in os.listdir(folder_path):
        folder = os.path.join(folder_path,path)
        for root,paths,files in os.walk(folder):
            for filename in files:
                if filename.endswith('.bag'):
                    bag_path = os.path.join(folder, filename)
                    for topic ,msg ,t in rosbag.Bag(bag_path).read_messages():
                        if "/A/netcom_gateway_master/gnss_data" == topic:
                            lat = msg.gnssrawdatainterface_m_latitude  # 假设消息中有latitude属性
                            lon = msg.gnssrawdatainterface_m_longitude  # 假设消息中有longitude属性
                            # 转换为GCJ02坐标
                            gcj_lat, gcj_lon = wgs84_to_gcj02(lat, lon)
                            results[filename] = {'gcj_latitude': gcj_lat, 'gcj_longitude': gcj_lon}
                            print(topic)
                            break

    return results

def single_check_bag_files_and_extract_data(file_path):
    results = {}
    for root,paths,files in os.walk(file_path):
        for filename in files:
            if filename.endswith('.bag'):
                bag_path = os.path.join(root, filename)
                for topic ,msg ,t in rosbag.Bag(bag_path).read_messages():
                    if "/A/netcom_gateway_master/gnss_data" == topic:
                        lat = msg.gnssrawdatainterface_m_latitude  # 假设消息中有latitude属性
                        lon = msg.gnssrawdatainterface_m_longitude  # 假设消息中有longitude属性
                        # 转换为GCJ02坐标
                        gcj_lat, gcj_lon = wgs84_to_gcj02(lat, lon)
                        results[filename] = {'gcj_latitude': gcj_lat, 'gcj_longitude': gcj_lon}
                        break

    return results